
#include <math.h>
#include <stdio.h>
#include "geodesy.h"
#include "constants.h"


/*************************************************************************************************/
static BOOL GEODESY_GetReferenceEllipseParameters_A_E2( 
  const GEODESY_enumReferenceEllipse ellipse,
  double* a,      // semi-major axis of the reference ellipse                     [m]
  double* e2      // eccentricity of the reference ellipse (e2 = (a*a-b*b)/(a*a)) [] 
  );

static BOOL GEODESY_GetReferenceEllipseParameters_A_B_E2( 
  const GEODESY_enumReferenceEllipse ellipse,
  double* a,      // semi-major axis of the reference ellipse                     [m]
  double* b,      // semi-minor axis of the reference ellipse                     [m]
  double* e2      // eccentricity of the reference ellipse (e2 = (a*a-b*b)/(a*a)) [] 
  );

static BOOL GEODESY_IsLatitudeValid( 
  const double latitude 
  );
/*************************************************************************************************/




BOOL GEODESY_GetReferenceEllipseParameters( 
  const GEODESY_enumReferenceEllipse ellipse, 
  double* a,      
  double* b,      
  double* f_inv,  
  double* e2      
  )
{
  switch( ellipse )
  {
  case GEODESY_REFERENCE_ELLIPSE_AIRY_1830: 
    *a     = GEODESY_REFERENCE_ELLIPSE_AIRY_1830_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_AIRY_1830_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_AIRY_1830_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_AIRY_1830_E2;
    break;
    
  case GEODESY_REFERENCE_ELLIPSE_MODIFED_AIRY:
    *a     = GEODESY_REFERENCE_ELLIPSE_MODIFED_AIRY_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_MODIFED_AIRY_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_MODIFED_AIRY_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_MODIFED_AIRY_E2;
    break;
    
  case GEODESY_REFERENCE_ELLIPSE_AUSTRALIAN_NATIONAL:
    *a     = GEODESY_REFERENCE_ELLIPSE_AUSTRALIAN_NATIONAL_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_AUSTRALIAN_NATIONAL_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_AUSTRALIAN_NATIONAL_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_AUSTRALIAN_NATIONAL_E2;
    break;
    
  case GEODESY_REFERENCE_ELLIPSE_BESSEL_1841:
    *a     = GEODESY_REFERENCE_ELLIPSE_BESSEL_1841_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_BESSEL_1841_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_BESSEL_1841_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_BESSEL_1841_E2;
    break;
    
  case GEODESY_REFERENCE_ELLIPSE_CLARKE_1866:
    *a     = GEODESY_REFERENCE_ELLIPSE_CLARKE_1866_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_CLARKE_1866_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_CLARKE_1866_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_CLARKE_1866_E2;
    break;
    
  case GEODESY_REFERENCE_ELLIPSE_CLARKE_1880:
    *a     = GEODESY_REFERENCE_ELLIPSE_CLARKE_1880_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_CLARKE_1880_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_CLARKE_1880_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_CLARKE_1880_E2;
    break;
    
  case GEODESY_REFERENCE_ELLIPSE_EVEREST_INDIA_1830:
    *a     = GEODESY_REFERENCE_ELLIPSE_EVEREST_INDIA_1830_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_EVEREST_INDIA_1830_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_EVEREST_INDIA_1830_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_EVEREST_INDIA_1830_E2;
    break;
    
  case GEODESY_REFERENCE_ELLIPSE_EVEREST_BRUNEI_E_MALAYSIA:
    *a     = GEODESY_REFERENCE_ELLIPSE_EVEREST_BRUNEI_E_MALAYSIA_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_EVEREST_BRUNEI_E_MALAYSIA_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_EVEREST_BRUNEI_E_MALAYSIA_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_EVEREST_BRUNEI_E_MALAYSIA_E2;
    break;
    
  case GEODESY_REFERENCE_ELLIPSE_EVEREST_W_MALAYSIA_SINGAPORE:
    *a     = GEODESY_REFERENCE_ELLIPSE_EVEREST_W_MALAYSIA_SINGAPORE_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_EVEREST_W_MALAYSIA_SINGAPORE_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_EVEREST_W_MALAYSIA_SINGAPORE_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_EVEREST_W_MALAYSIA_SINGAPORE_E2;
    break;
    
  case GEODESY_REFERENCE_ELLIPSE_GRS_1980:
    *a     = GEODESY_REFERENCE_ELLIPSE_GRS_1980_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_GRS_1980_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_GRS_1980_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_GRS_1980_E2;
    break;
    
  case GEODESY_REFERENCE_ELLIPSE_HELMERT_1906:
    *a     = GEODESY_REFERENCE_ELLIPSE_HELMERT_1906_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_HELMERT_1906_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_HELMERT_1906_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_HELMERT_1906_E2;
    break;
    
  case GEODESY_REFERENCE_ELLIPSE_HOUGH_1960:
    *a     = GEODESY_REFERENCE_ELLIPSE_HOUGH_1960_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_HOUGH_1960_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_HOUGH_1960_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_HOUGH_1960_E2;
    break;
    
  case GEODESY_REFERENCE_ELLIPSE_INTERNATIONAL_1924:
    *a     = GEODESY_REFERENCE_ELLIPSE_INTERNATIONAL_1924_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_INTERNATIONAL_1924_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_INTERNATIONAL_1924_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_INTERNATIONAL_1924_E2;
    break;
    
  case GEODESY_REFERENCE_ELLIPSE_SOUTH_AMERICAN_1969:
    *a     = GEODESY_REFERENCE_ELLIPSE_SOUTH_AMERICAN_1969_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_SOUTH_AMERICAN_1969_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_SOUTH_AMERICAN_1969_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_SOUTH_AMERICAN_1969_E2;
    break;
    
  case GEODESY_REFERENCE_ELLIPSE_WGS72:
    *a     = GEODESY_REFERENCE_ELLIPSE_WGS72_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_WGS72_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_WGS72_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_WGS72_E2;
    break;
    
  case GEODESY_REFERENCE_ELLIPSE_WGS84:
    *a     = GEODESY_REFERENCE_ELLIPSE_WGS84_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_WGS84_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_WGS84_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_WGS84_E2;
    break;
  case GEODESY_REFERENCE_ELLIPSE_PZ90:
    *a     = GEODESY_REFERENCE_ELLIPSE_PZ90_A;
    *f_inv = GEODESY_REFERENCE_ELLIPSE_PZ90_F_INV;
    *b     = GEODESY_REFERENCE_ELLIPSE_PZ90_B;
    *e2    = GEODESY_REFERENCE_ELLIPSE_PZ90_E2;
    break;

  default:
    printf( "Unexpected default case." );
    return FALSE;
    break;  
  }
  return TRUE;
}


// static
BOOL GEODESY_GetReferenceEllipseParameters_A_E2( 
  const GEODESY_enumReferenceEllipse ellipse,
  double* a,      
  double* e2     
  )
{  
  double b;
  double f_inv;  
  BOOL result;

  result = GEODESY_GetReferenceEllipseParameters( 
    ellipse,
    a,
    &b,
    &f_inv,
    e2 );

  return result;
}


// static
BOOL GEODESY_GetReferenceEllipseParameters_A_B_E2( 
  const GEODESY_enumReferenceEllipse ellipse,
  double* a,  
  double* b, 
  double* e2
  )
{  
  double f_inv;  
  BOOL result;

  result = GEODESY_GetReferenceEllipseParameters( 
    ellipse,
    a,
    b,
    &f_inv,
    e2 );

  return result;
}

// static
BOOL GEODESY_IsLatitudeValid( 
  const double latitude 
  )
{
  // check for valid latitude out of range
  if( latitude > HALFPI || latitude < -HALFPI )  
  {
    printf( "if( latitude > HALFPI || latitude < -HALFPI )" );
    return FALSE;
  }
  else
  {
    return TRUE;
  }
}







BOOL GEODESY_ConvertGeodeticCurvilinearToEarthFixedCartesianCoordinates(
  const GEODESY_enumReferenceEllipse  referenceEllipse,  
  const double latitude,  
  const double longitude, 
  const double height,    
  double *x,              
  double *y,             
  double *z             
  )
{  
  double a;      
  double e2;    
  double N;    
  double sinlat;
  double dtmp;  
  BOOL result;

  // get necessary reference ellipse parameters
  result = GEODESY_GetReferenceEllipseParameters_A_E2( referenceEllipse, &a, &e2 );
  if( result == FALSE )
  {
    *x = 0.0;
    *y = 0.0;
    *z = 0.0;
    printf( "Reference ellipse invalid." );
    return FALSE;
  }
  
  // check for valid latitude out of range
  result = GEODESY_IsLatitudeValid( latitude );
  if( result == FALSE )
  {
    *x = 0.0;
    *y = 0.0;
    *z = 0.0;
    printf( "Input latitude is invalid." );
    return FALSE;
  }

  sinlat = sin( latitude );             
  N = a / sqrt( 1.0 - e2 * sinlat*sinlat );      
  dtmp = (N + height) * cos(latitude);

  *x = dtmp * cos(longitude);
  *y = dtmp * sin(longitude);
  *z = ( (1.0 - e2)*N + height ) * sinlat;

  return TRUE;
}




BOOL GEODESY_ConvertEarthFixedCartesianToGeodeticCurvilinearCoordinates(
  const GEODESY_enumReferenceEllipse  referenceEllipse,  
  const double x,              // earth fixed cartesian coordinate [m]
  const double y,              // earth fixed cartesian coordinate [m]
  const double z,              // earth fixed cartesian coordinate [m]  
  double *latitude,            // geodetic latitude              [rad]
  double *longitude,           // geodetic longitude             [rad]
  double *height               // geodetic height                [m]
  )
{
  double a;      // semi-major axis of reference ellipse [m]
  double b;      // semi-minor axis of reference ellipse [m]
  double e2;     // first eccentricity of reference ellipse []
  double N;      // prime vertical radius of curvature [m]
  double p;      // sqrt( x^2 + y^2 ) [m]
  double dtmp;   // temp
  double sinlat; // sin(lat)
  double lat;    // temp geodetic latitude  [rad]
  double lon;    // temp geodetic longitude [rad]
  double hgt;    // temp geodetic height    [m]
  BOOL result;
  
  // get necessary reference ellipse parameters
  result = GEODESY_GetReferenceEllipseParameters_A_B_E2( referenceEllipse, &a, &b, &e2 );
  if( result == FALSE )
  {
    *latitude  = 0;
    *longitude = 0;  
    *height    = 0;  
    printf( "Reference ellipse invalid." );    
    return FALSE;
  }
  
  if( x == 0.0 && y == 0.0 ) 
  {
    // at a pole    
    // most likely to happen while using a simulator
    
    // longitude is really unknown
    lon = 0.0; 
    
    if( z < 0 )
    {
      hgt = -z - b;
      lat = -HALFPI;
    }
    else
    {
      hgt = z - b;
      lat = HALFPI;
    }
  }
  else
  {
    p = sqrt( x*x + y*y );

    // unique solution for longitude
    // best formula for any longitude and applies well near the poles
    lon = 2.0 * atan2( y , ( x + p ) );
    
    // set approximate initial latitude assuming a height of 0.0
    lat = atan( z / (p * (1.0 - e2)) );
    hgt = 0.0;
    do
    { 
      dtmp = hgt;
      sinlat = sin(lat);
      N   = a / sqrt( 1.0 - e2*sinlat*sinlat );
      hgt = p / cos(lat) - N;
      lat = atan( z / (p * ( 1.0 - e2*N/(N + hgt) )) );      

    } while( fabs( hgt - dtmp ) > 0.0001 );  
  }

  *latitude  = lat;
  *longitude = lon;  
  *height    = hgt;
  return TRUE;
}


BOOL GEODESY_ComputeNorthingEastingVertical(
  const GEODESY_enumReferenceEllipse  referenceEllipse,  
  const double referenceLatitude,  
  const double referenceLongitude, 
  const double referenceHeight,    
  const double latitude,           
  const double longitude,         
  const double height,            
  double *northing,               
  double *easting,                
  double *vertical               
  )
{
  double x_ref;
  double y_ref;
  double z_ref;
  double x;
  double y;
  double z;
  double dx;
  double dy;
  double dz;
  double A;   // rotation angle [rad]
  double B;   // rotation angle [rad]
  double cosA;
  double sinA;
  double cosB;
  double sinB;
  BOOL result;

  *northing = 0;
  *easting  = 0;
  *vertical = 0;

  result = GEODESY_IsLatitudeValid( referenceLatitude );
  if( result == FALSE )
  {
    printf( "Input reference latitude is invalid" );
    return FALSE;  
  }
  result = GEODESY_IsLatitudeValid( latitude );  
  if( result == FALSE )
  {
    printf( "Input latitude is invalid." );
    return FALSE;  
  }
  
  result = GEODESY_ConvertGeodeticCurvilinearToEarthFixedCartesianCoordinates(
    referenceEllipse,
    referenceLatitude,
    referenceLongitude,
    referenceHeight,
    &x_ref,
    &y_ref,
    &z_ref );
  if( result == FALSE )
  {
    printf( "GEODESY_ConvertGeodeticCurvilinearToEarthFixedCartesianCoordinates return FALSE." );
    return FALSE;
  }
  
  result = GEODESY_ConvertGeodeticCurvilinearToEarthFixedCartesianCoordinates(
    referenceEllipse,
    latitude,
    longitude,
    height,
    &x,
    &y,
    &z );
  if( result == FALSE )
  {
    printf( "GEODESY_ConvertGeodeticCurvilinearToEarthFixedCartesianCoordinates return FALSE." );
    return FALSE;
  }

  // A and B are rotation angles
  A = referenceLatitude - HALFPI;
  B = referenceLongitude - PI;

  cosA = cos(A);
  sinA = sin(A);
  cosB = cos(B);
  sinB = sin(B);

  // the cartesian vector between the two points in the geodetic 
  // frame is rotated to the local geodetic frame
  dx = x - x_ref;
  dy = y - y_ref;
  dz = z - z_ref;   

  *northing = cosA*cosB * dx  +  cosA*sinB * dy  -  sinA*dz;
  *easting  = sinB      * dx  -  cosB      * dy;
  *vertical = sinA*cosB * dx  +  sinA*sinB * dy  +  cosA*dz;   
  return TRUE;
}


BOOL GEODESY_ComputePositionDifference(
  const GEODESY_enumReferenceEllipse  referenceEllipse,  
  const double referenceLatitude,  
  const double referenceLongitude, 
  const double referenceHeight,    
  const double latitude,           
  const double longitude,          
  const double height,             
  double *difference_northing,     
  double *difference_easting,      
  double *difference_vertical      
  )
{
  BOOL result;
  result = GEODESY_ComputeNorthingEastingVertical(
    referenceEllipse,
    referenceLatitude, 
    referenceLongitude,
    referenceHeight,   
    latitude,          
    longitude,         
    height,            
    difference_northing,
    difference_easting,
    difference_vertical );  
  return result;
}




BOOL GEODESY_ComputeMeridianRadiusOfCurvature(
  const GEODESY_enumReferenceEllipse  referenceEllipse, 
  const double latitude,  
  double*  M              
  )
{
  double a;  // semi-major axis of reference ellipse    [m]
  double e2; // first eccentricity of reference ellipse []
  double dtmp;
  BOOL result;
  
  // get necessary reference ellipse parameters
  result = GEODESY_GetReferenceEllipseParameters_A_E2( referenceEllipse, &a, &e2 );
  if( result == FALSE )
  {
    *M = 0;
    printf( "Reference ellipse invalid." );    
    return result;
  }
    
  dtmp = sin(latitude);
  dtmp = sqrt( 1.0 - e2 * dtmp * dtmp );  // W
  dtmp = dtmp*dtmp*dtmp;                  // W^3    
  
  *M = a * ( 1.0 - e2 ) / dtmp;
  return TRUE;
}





BOOL GEODESY_ComputePrimeVerticalRadiusOfCurvature(
  const GEODESY_enumReferenceEllipse  referenceEllipse, 
  const double latitude,  
  double*  N              
  )
{
  double a;  // semi-major axis of reference ellipse [m]
  double e2; // first eccentricity of reference ellipse []
  double W;
  BOOL result;
  
  // get necessary reference ellipse parameters
  result = GEODESY_GetReferenceEllipseParameters_A_E2( referenceEllipse, &a, &e2 );
  if( result == FALSE )
  {
    *N = 0;
    printf( "Reference ellipse invalid." );    
    return result;  
  }
  
  W = sin(latitude);
  W = sqrt( 1.0 - e2 * W * W );      
  
  *N = a / W;
  return TRUE;
}


BOOL GEODESY_ComputeMeridianArcBetweenTwoLatitudes(
  const GEODESY_enumReferenceEllipse  referenceEllipse, 
  const double referenceLatitude,  
  const double latitude,           
  double*      arc                 
  )
{
  double a;  // semi-major axis of reference ellipse [m]
  double e2; // first eccentricity of reference ellipse []
  double e4; 
  double e6;
  double e8;
  double dtmp;
  double A;
  double B;
  double C;
  double D; 
  double E;
  double arc_ref; // arc from equator for the reference lat [m]
  double arc_p;   // arc from eqautor for point 'P' [m]
  BOOL result;

  *arc = 0;

  result = GEODESY_IsLatitudeValid( referenceLatitude );
  if( result == FALSE )
  {
    printf( "Reference latitude is invalid." );
    return result;
  }
  
  // get necessary reference ellipse parameters
  result = GEODESY_GetReferenceEllipseParameters_A_E2( referenceEllipse, &a, &e2 );
  if( result == FALSE )
  {
    printf( "Reference ellipse invalid." );    
    return result;  
  }
    
  e4 = e2*e2;
  e6 = e4*e2;
  e8 = e6*e2;
  dtmp = a*(1.0-e2);

  A =  dtmp * ( 1.0 + 0.75  * e2 + 0.703125   * e4 + 0.68359375     * e6 + 0.67291259765625 * e8 ); //  dtmp * (1.0 + 3.0/4.0*e2 + 45.0/64.0*e4  + 175.0/256.0*e6  + 11025.0/16384.0*e8 );
  B = -dtmp * (       0.375 * e2 + 0.46875    * e4 + 0.5126953125   * e6 + 0.538330078125   * e8 ); // -dtmp * (      3.0/8.0*e2 + 15.0/32.0*e4  + 525.0/1024.0*e6 + 2205.0/4096.0*e8 );
  C =  dtmp * (                    0.05859375 * e4 + 0.1025390625   * e6 + 0.13458251953125 * e8 ); // -dtmp * (                   15.0/256.0*e4 + 105.0/1024.0*e6 + 2205.0/16384.0*e8 );  
  D = -dtmp * (                                      0.011393229167 * e6 + 0.025634765625   * e8 ); // -dtmp * (                                   35.0/3072.0*e6  + 105.0/4096.0*e8 );  
  E =  dtmp * ( 2.4032593e-03 * e8 );                                                             

  arc_ref = A*referenceLatitude + B*sin(2.0*referenceLatitude) + C*sin(4.0*referenceLatitude) + D*sin(6.0*referenceLatitude) + E*sin(8.0*referenceLatitude);
  arc_p = A*latitude + B*sin(2.0*latitude) + C*sin(4.0*latitude) + D*sin(6.0*latitude) + E*sin(8.0*latitude);

  *arc = arc_p - arc_ref;
  return TRUE;
}




BOOL GEODESY_ComputeParallelArcBetweenTwoLongitudes(
  const GEODESY_enumReferenceEllipse  referenceEllipse, 
  const double referenceLatitude,  
  const double referenceLongitude, 
  const double longitude,          
  double*      arc                 
  )
{
  double a;  // semi-major axis of reference ellipse [m]
  double e2; // first eccentricity of reference ellipse []
  double N;  // computed prime vertical radius of curvature [m]
  BOOL result;

  *arc = 0;
  
  // get necessary reference ellipse parameters
  result = GEODESY_GetReferenceEllipseParameters_A_E2( referenceEllipse, &a, &e2 );
  if( result == FALSE )
  {
    printf( "Reference ellipse invalid." );    
    return result;    
  }

  result = GEODESY_IsLatitudeValid( referenceLatitude );
  if( result == FALSE )
  {
    printf( "Reference latitude is invalid." );
    return result;    
  }
  
  N = sin(referenceLatitude);
  N = a / sqrt( 1.0 - e2 * N * N );

  *arc = N * cos(referenceLatitude) * (longitude - referenceLongitude);  
  return TRUE;
}




BOOL GEODESY_RotateVectorFromLocalGeodeticFrameToEarthFixedFrame(
  const double referenceLatitude,  
  const double referenceLongitude, 
  const double dN,                 
  const double dE,                 
  const double dUp,                
  double* dX,                      
  double* dY,                      
  double* dZ                       
  )
{
  double sinlat;
  double coslat;
  double sinlon;
  double coslon;
  BOOL result;

  result = GEODESY_IsLatitudeValid( referenceLatitude );
  if( result == FALSE )
  {
    *dX = 0;
    *dY = 0;
    *dZ = 0;
    printf( "Reference latitude is invalid." );
    return result;
  }

  sinlat = sin(referenceLatitude);
  coslat = cos(referenceLatitude);
  sinlon = sin(referenceLongitude);
  coslon = cos(referenceLongitude);
  
  *dX = -sinlat*coslon * dN  -  sinlon * dE  +  coslat*coslon * dUp;
  *dY = -sinlat*sinlon * dN  +  coslon * dE  +  coslat*sinlon * dUp;
  *dZ =  coslat        * dN                  +  sinlat        * dUp;
  return TRUE;
}




BOOL GEODESY_RotateVectorFromEarthFixedFrameToLocalGeodeticFrame(
  const double referenceLatitude,  
  const double referenceLongitude, 
  const double dX,                 
  const double dY,                 
  const double dZ,                 
  double* dN,                      
  double* dE,                      
  double* dUp                      
  )
{
  double sinlat;
  double coslat;
  double sinlon;
  double coslon;
  BOOL result;

  result = GEODESY_IsLatitudeValid( referenceLatitude );
  if( result == FALSE )
  {
    *dN = 0;
    *dE = 0;
    *dUp = 0;
    printf( "Reference latitude is invalid." );
    return result;    
  }

  sinlat = sin(referenceLatitude);
  coslat = cos(referenceLatitude);
  sinlon = sin(referenceLongitude);
  coslon = cos(referenceLongitude);
  
  *dN  = -sinlat*coslon * dX  -  sinlat*sinlon * dY  +  coslat * dZ;
  *dE  = -sinlon        * dX  +  coslon        * dY;
  *dUp =  coslat*coslon * dX  +  coslat*sinlon * dY  +  sinlat * dZ;  

  return TRUE;
}



BOOL GEODESY_ComputeAzimuthAndElevationAnglesBetweenToPointsInTheEarthFixedFrame(
  const GEODESY_enumReferenceEllipse  referenceEllipse, 
  const double fromX, 
  const double fromY, 
  const double fromZ, 
  const double toX,   
  const double toY,   
  const double toZ,   
  double* elevation,  
  double* azimuth     
  )
{
  double lat; // reference geodetic latitude  ('from' point) [rad]
  double lon; // reference geodetic longitude ('from' point) [rad]
  double dX;  // ECEF X vector component between 'from' and 'to' point (m)
  double dY;  // ECEF Y vector component between 'from' and 'to' point (m)
  double dZ;  // ECEF Z vector component between 'from' and 'to' point (m)
  double dN;  // LG northing vector component between 'from' and 'to' point (m)
  double dE;  // LG easting  vector component between 'from' and 'to' point (m)
  double dUp; // LG vertical vector component between 'from' and 'to' point (m)
  double tmp; // temp value
  BOOL result;

  *elevation = 0;
  *azimuth = 0; 

  // get the reference geodetic curvilinear coordinates from the 'from' point
  result = GEODESY_ConvertEarthFixedCartesianToGeodeticCurvilinearCoordinates(
    referenceEllipse,
    fromX,
    fromY,
    fromZ,
    &lat,
    &lon,
    &tmp );
  if( result == FALSE )
  {
    printf( "GEODESY_ConvertEarthFixedCartesianToGeodeticCurvilinearCoordinates returned FALSE." );
    return result;      
  }

  // vector between the two points in the earth fixed frame
  dX = toX - fromX;
  dY = toY - fromY;
  dZ = toZ - fromZ;

  // rotate the vector to the local geodetic frame
  result = GEODESY_RotateVectorFromEarthFixedFrameToLocalGeodeticFrame(
    lat,
    lon,
    dX,
    dY,
    dZ,
    &dN,
    &dE,
    &dUp );
  if( result == FALSE )
  {
    printf( "GEODESY_RotateVectorFromEarthFixedFrameToLocalGeodeticFrame returned FALSE." );
    return result;    
  }
  
  // compute the elevation
  tmp = sqrt( dN*dN + dE*dE );
  *elevation = atan( dUp / tmp );
  
  // compute the azimuth
  *azimuth = atan2(dE, dN);

  // by convention, azimuth will be between 0 to 2 PI
  if( *azimuth < 0.0 )
    *azimuth += TWOPI;
  
  return TRUE;
}

